#include "rtc5agent.h"
#include "RTC5impl.h"
#include "log/log.h"
#include <limits>
#include <cmath>
#include <QObject>
#include <QDebug>
#include "../slice/cliread.h"
#include "../system/taskagent.h"

namespace HIM {

#define     CROSSSIZE           150
#define     MATH_PI             3.1415926535897931
#define     MY_ESP              0.0001
#define     LIST_SWITCH         490000

RTC5Agent::RTC5Agent()
{

}

RTC5Agent::~RTC5Agent()
{      
    Free();
}

bool RTC5Agent::init()
{
    initSuccess = false;

    if(Start()){
        initSuccess=true;
    }

    HIM_DEBUG("RTC5Agent init initSuccess--- {}",initSuccess);
    return initSuccess;
}

bool RTC5Agent::Start()
{
    HIM_INFO("初始化RTC板卡");

    UINT ErrorCode(init_rtc5_dll());

    if (ErrorCode){
        QString errStr ="Error PCIe's DLL initialization #";
        getErrStr(ErrorCode,errStr);
        HIM_ERROR(errStr.toStdString());
        return false;
    }

    // Initialize the RTC5
    n_stop_execution(laserAIndex);
    ErrorCode = load_program_file("../data/rtc5");
    if (ErrorCode){
        QString errStr ="PCIe Program file loading error #";
        switch (ErrorCode) {
        case 1 :{
            errStr.append("eset error: the board could not be reset.");
        }break;
        case 2 :{
            errStr.append("Unreset error: the board does not restart.");
        }break;
        case 3 :{
            errStr.append("File error: file not found or cannot be opened.");
        }break;
        case 4 :{
            errStr.append("Format error: RTC5OUT.out has incorrect format.");
        }break;
        case 5 :{
            errStr.append("System error: not enough Windows memory.");
        }break;
        case 6 :{
            errStr.append("Access error: board reserved for another user program.");
        }break;
        case 7 :{
            errStr.append("Version error: RTC5 DLL version, RTC5RBF.rbf version and RTC5OUT."
                          "out version are not compatible with each other");
        }break;
        case 8 :{
            errStr.append("RTC5 board driver not found (get_last_error return code RTC5_ACCESS_DENIED).");
        }break;
        case 9 :{
            errStr.append("DriverCall error: loading of RTC5OUT.out file failed.");
        }break;
        case 10 :{
            errStr.append("Configuration error: DSP register initialization failed.");
        }break;
        case 11 :{
            errStr.append("Firmware error: loading of RTC5RBF.rbf file failed.");
        }break;
        case 12 :{
            errStr.append("PCI download error: loading of RTC5DAT.dat file failed.");
        }break;
        case 13 :{
            errStr.append("Busy error: Download locked, board is BUSY or"
                          "INTERNAL-BUSY list execution status (get_last_error return code RTC5_BUSY).");
        }break;
        case 14 :{
            errStr.append("DSP memory error");
        }break;
        case 15 :{
            errStr.append("Verify error (only applicable for download verification).");
        }break;
        case 16 :{
            errStr.append("PCI error.");
        }break;
        default:{
            errStr.append("unknown.");
        }break;
        }
        HIM_ERROR(errStr.toStdString());
        return false;
    }

    if (CheckError())
        n_reset_error(laserAIndex, ErrorCode);

    UINT NoOfSelectedCard = select_rtc(laserAIndex);

    if ( NoOfSelectedCard != laserAIndex){
        QString errStr ="Error: Failed to select the 1st PCIe.";
        HIM_ERROR(errStr.toStdString());
        return false;
    }

    int ountCards = (int)rtc5_count_cards();
    int dllVer = (int)get_dll_version();
    int hecVer = (int)get_hex_version();
    unsigned int rtcVer = get_rtc_version();
    unsigned int numSer = n_get_serial_number(laserAIndex);
    bool IsMOTF = (bool)(rtcVer & 256U);
    bool Is2ndHead = (bool)(rtcVer & 512U);

    string rtc5Info{""};
    rtc5Info.append("ountCards:").append(to_string(ountCards))
            .append("dllVer:").append(to_string(dllVer))
            .append("hecVer:").append(to_string(hecVer))
            .append("rtcVer:").append(to_string(rtcVer))
            .append("numSer:").append(to_string(numSer))
            .append("IsMOTF:").append(to_string(IsMOTF))
            .append("Is2ndHead:").append(to_string(Is2ndHead));

    HIM_INFO(rtc5Info);

    // 设置信号的有效电平
    // Bit #6 Synchronization is switched on
    n_set_laser_control(laserAIndex, 0U);

    //todo 切换Ct5文件路径
    if (!LoadCt5(laserCalibrationFilePath, 0)){
        string errStr ="Error: Failed Load:";
        errStr.append(laserCalibrationFilePath);
        HIM_ERROR(errStr);
        return false;
    }

    //1048576 = 2^20
    n_config_list(laserAIndex,LIST1_2_CONUT, LIST1_2_CONUT);

    // Laser Mode 4 is used for some fiber lasers
    // = 1: YAG Mode 1.
    n_set_laser_mode(laserAIndex, 4U);
    n_set_firstpulse_killer(laserAIndex, 200*8);
    //If HalfPeriod = 0 and/or PulseLength = 0, no laser signals are outputted
    n_set_laser_pulses_ctrl(laserAIndex, 0U, 0U);
    //P175
    //deactivated (turned off) by setting the standby pulse length and/or the standby output period to zero(default)
    n_set_standby(laserAIndex, 0U, 0U);

    // 打印完回原点
    ctlSetHomePosition(0, 0);
    ctlEnableLaser(false);
    return true;
}

bool RTC5Agent::LoadCt5(string ct5path, int tableIndex)
{
    unsigned int ErrorCode;
    ErrorCode = n_load_correction_file(laserAIndex,
                                       ct5path.c_str(),
                                       (UINT)(tableIndex + 1),
                                       2U);
    if (ErrorCode){
        QString errStr ="Correction file loading error #";
        switch (ErrorCode) {
        case 1 :{
            errStr.append("File error (file corrupt or incomplete).");
        }break;
        case 2 :{
            errStr.append("Memory error (RTC5 DLL-internal, Windows system memory).");
        }break;
        case 3 :{
            errStr.append("File-open error (empty string submitted for Name parameter, file not found, etc.).");
        }break;
        case 4 :{
            errStr.append("DSP memory error.");
        }break;
        case 5 :{
            errStr.append("PCI download error (RTC5 board driver error).");
        }break;
        case 8 :{
            errStr.append("RTC5 board driver not found (get_last_error return code RTC5_ACCESS_DENIED).");
        }break;
        case 10 :{
            errStr.append("Parameter error (incorrect No).");
        }break;
        case 11 :{
            errStr.append("Access error: board reserved for another user program.");
        }break;
        case 12 :{
            errStr.append("Warning: 3D correction table or Dim = 3 selected, but the Option “3D” is not "
                          " enabled. The system subsequently operates as an ordinary 2D system (this "
                          "warning is only returned, if no other error has occurred).");
        }break;
        case 13 :{
            errStr.append("Busy error: no download, board is BUSY or INTERNAL-BUSY list execution status.");
        }break;
        case 14 :{
            errStr.append("PCI upload error (RTC5 board driver error, only with download verification). ");
        }break;
        case 15 :{
            errStr.append("Verify error (only with download verification).");
        }break;
        default:{
            errStr.append("unknown.");
        }break;
        }
        HIM_ERROR(errStr.toStdString());
        return false;
    }

    n_select_cor_table(laserAIndex, 1U, 0U);

    if (CheckError())
        return false;

    return true;
}

void RTC5Agent::Free()
{
    if (initSuccess){
        free_rtc5_dll();
        initSuccess = false;
    }

    HIM_DEBUG("RTC5Agent Free initSuccess--- {}",initSuccess);
}

void RTC5Agent::loadAllLaserConfig()
{   
    sizeXY = SYSTEMCONFIG.getLongValue(PARAMETER,"sizeXY");

    kFactor = SYSTEMCONFIG.getDoubleValue(PARAMETER,"laserKFactor");

    laserCalibrationFilePath = SYSTEMCONFIG.getValue(PARAMETER,"laserCalibrationFilePath");

    laserPowerMax = SYSTEMCONFIG.getLongValue(PARAMETER,"laserPowerMax");
    laserTestPower = SYSTEMCONFIG.getLongValue(PARAMETER,"laserTestPower");
    laserPulseWidth = SYSTEMCONFIG.getLongValue(PARAMETER,"laserPulseWidth");
    laserFrequency = SYSTEMCONFIG.getLongValue(PARAMETER,"laserFrequency");
    laserJumpSpeed = SYSTEMCONFIG.getLongValue(PARAMETER,"laserJumpSpeed");
    laserMarkSpeed = SYSTEMCONFIG.getLongValue(PARAMETER,"laserMarkSpeed");

    laserJumpDelay = SYSTEMCONFIG.getLongValue(PARAMETER,"laserJumpDelay");
    laserMarkDelay = SYSTEMCONFIG.getLongValue(PARAMETER,"laserMarkDelay");
    laserPolyDelay = SYSTEMCONFIG.getLongValue(PARAMETER,"laserPolyDelay");
    laserOnDelay = SYSTEMCONFIG.getLongValue(PARAMETER,"laserOnDelay");
    laserOffDelay = SYSTEMCONFIG.getLongValue(PARAMETER,"laserOffDelay");

    laserSkyWritingTimelag = SYSTEMCONFIG.getLongValue(PARAMETER,"laserSkyWritingTimelag");
    laserSkyWritingNprev = SYSTEMCONFIG.getLongValue(PARAMETER,"laserSkyWritingNprev");
    laserSkyWritingOnShift = SYSTEMCONFIG.getLongValue(PARAMETER,"laserSkyWritingOnShift");
    laserSkyWritingNpost = SYSTEMCONFIG.getLongValue(PARAMETER,"laserSkyWritingNpost");
    laserSkyWritingAngle = SYSTEMCONFIG.getLongValue(PARAMETER,"laserSkyWritingAngle");

    laserPowerByLen2d0 = SYSTEMCONFIG.getDoubleValue(PARAMETER,"laserPowerByLen2d0");
    laserPowerByLen1d0 = SYSTEMCONFIG.getDoubleValue(PARAMETER,"laserPowerByLen1d0");
    laserPowerByLen0d5 = SYSTEMCONFIG.getDoubleValue(PARAMETER,"laserPowerByLen0d5");
    laserPowerByLen0d2 = SYSTEMCONFIG.getDoubleValue(PARAMETER,"laserPowerByLen0d2");

    laserXmove = SYSTEMCONFIG.getDoubleValue(PARAMETER,"laserXmove");
    laserYmove = SYSTEMCONFIG.getDoubleValue(PARAMETER,"laserYmove");
    laserXScale = SYSTEMCONFIG.getDoubleValue(PARAMETER,"laserXScale");
    laserYScale = SYSTEMCONFIG.getDoubleValue(PARAMETER,"laserYScale");
    laserXYrotate = SYSTEMCONFIG.getDoubleValue(PARAMETER,"laserXYrotate");
}

void RTC5Agent::ApplyLaserConfig()
{    
    ctlSetFrequency(laserFrequency , laserPulseWidth);
    ctlSetScaleAngle(laserXScale, laserYScale, laserXYrotate);
    ctlSetOffsetPosition(laserXmove, laserYmove, laserXYrotate);
    ctrlSetJumpSpeed(laserJumpSpeed);
    MarkStart();
    listSetDelayMode(laserJumpDelay);
    listSetDelayTime(laserOnDelay,laserOffDelay,laserJumpDelay,laserMarkDelay,laserPolyDelay);
    listSetSkyWritring(laserSkyWritingTimelag,laserSkyWritingNprev,laserSkyWritingOnShift,
                       laserSkyWritingNpost,laserSkyWritingAngle);
    MarkEnd();
}

bool RTC5Agent::CheckError()
{
    bool isErr(false);

    UINT ErrorCode = n_get_last_error(laserAIndex);
    if(ErrorCode){
        isErr = true;
        QString errStr ="RTC board error #";
        getErrStr(ErrorCode,errStr);
        HIM_ERROR(errStr.toStdString());
    }

    UINT errHead = n_get_head_status(laserAIndex, 1U);
    if(!(errHead&(1<<3))){
        isErr = true;
        QString errStr ="RTC head error #";
        errStr.append("Position Acknowledge of x axis = false ;");
        HIM_ERROR(errStr.toStdString());
    }

    if(!(errHead&(1<<4))){
        isErr = true;
        QString errStr ="RTC head error #";
        errStr.append("Position Acknowledge of y axis = false ;");
        HIM_ERROR(errStr.toStdString());
    }

    if(!(errHead&(1<<6))){
        isErr = true;
        QString errStr ="RTC head error #";
        errStr.append("Temperature Status = false ;");
        HIM_ERROR(errStr.toStdString());
    }

    if(!(errHead&(1<<7))){
        isErr = true;
        QString errStr ="RTC head error #";
        errStr.append("Power Status = false ;");
        HIM_ERROR(errStr.toStdString());
    }

    return isErr;
}

void RTC5Agent::getErrStr(UINT errCode , QString& errStr)
{
    if(errCode&(1<<RTC5_NO_CARD)){
        errStr.append("RTC5_NO_CARD,");
    }

    if(errCode&(1<<RTC5_ACCESS_DENIED)){
        errStr.append("RTC5_ACCESS_DENIED,");
    }

    if(errCode&(1<<RTC5_SEND_ERROR)){
        errStr.append("RTC5_SEND_ERROR,");
    }

    if(errCode&(1<<RTC5_TIMEOUT)){
        errStr.append("RTC5_TIMEOUT,");
    }

    if(errCode&(1<<RTC5_PARAM_ERROR)){
        errStr.append("RTC5_PARAM_ERROR,");
    }

    if(errCode&(1<<RTC5_BUSY)){
        errStr.append("RTC5_BUSY,");
    }

    if(errCode&(1<<RTC5_REJECTED)){
        errStr.append("RTC5_REJECTED,");
    }

    if(errCode&(1<<RTC5_VERSION_MISMATCH)){
        errStr.append("RTC5_VERSION_MISMATCH,");
    }

    if(errCode&(1<<RTC5_TYPE_REJECTED)){
        errStr.append("RTC5_TYPE_REJECTED,");
    }

    if(errCode&(1<<RTC5_OUT_OF_MEMORY)){
        errStr.append("RTC5_OUT_OF_MEMORY,");
    }

    if(errCode&(1<<RTC5_EEPROM_ERROR)){
        errStr.append("RTC5_EEPROM_ERROR,");
    }

    if(errCode&(1<<RTC5_CONFIG_ERROR)){
        errStr.append("RTC5_CONFIG_ERROR,");
    }
}

bool RTC5Agent::ctlEnableLaser(bool en)
{
    if (CheckError())
        en = false;

    if (en)
        n_enable_laser(laserAIndex);
    else
        n_disable_laser(laserAIndex);

    return en;
}

void RTC5Agent::ctlSetHomePosition(double vX, double vY)
{
    int XHome = (int)(vX * kFactorX);
    int YHome = (int)(vY * kFactorY);
    n_home_position(laserAIndex, XHome, YHome);
}

void RTC5Agent::ctlSetScaleAngle(double sX , double sY , double a)
{
    n_set_angle(laserAIndex, 1U, a, 1);
    kFactorX = kFactor * sX;
    kFactorY = kFactor * sY;
}

void RTC5Agent::ctlSetOffsetPosition(double vX, double vY , double a )
{
    double A = a * MATH_PI / 180.0;
    double cosA = cos(A);
    double sinA = sin(A);
    int XOff = (int)((cosA * vX - sinA * vY) * kFactorX); //(int)(-vY * this.kFactorX);
    int YOff = (int)((sinA * vX + cosA * vY) * kFactorY);  //(int)(vX * this.kFactorY);
    n_set_offset(laserAIndex,1, XOff, YOff, 0);
}

void RTC5Agent::ctlSetLaserPinOut(unsigned int pins)
{
    //Output value (DIGITAL OUT1 and DIGITAL OUT2).
    // As an unsigned 32 - bit value.
    // Bit # 0: DIGITAL OUT1.  红光使能
    // Bit # 1: DIGITAL OUT2.
    n_set_laser_pin_out(laserAIndex, pins);
}

void RTC5Agent::ctlSetFrequency(INT32 frequency , UINT32 pulseWidth)
{
    // HZ -> 1/64us
    unsigned int halfPeriod = 1000000 * 32 / frequency;
    unsigned int pulseLength = halfPeriod * pulseWidth / 100;
    // 设置激光脉冲 Setting the standby pulses
    // In RTC4 compatibility mode the standby parameters are specified in units of 1/8 µs as with the RTC4 and the
    // RTC5 multiplies the specified values by 8 to convert in integer-multiple of 1/64 µs.
    // Half of the standby output period = 100 µs
    // Pulse length of the standby pulses = 1 µs
    // RTC5Wrap.set_standby(800, 80);
    n_set_standby(laserAIndex, halfPeriod, pulseLength);
    n_set_laser_pulses_ctrl(laserAIndex, halfPeriod, pulseLength);
    HIM_INFO("Laser standby:" + std::to_string(pulseLength)+ "/" + std::to_string(halfPeriod));
}

bool RTC5Agent::ctrlSetJumpSpeed(double jump)
{
    double jumpBits = jump * kFactor / 1000.0;

    if (jumpBits < 1.6)
        jumpBits = 1.6;
    if (jumpBits > 800000.0)
        jumpBits = 800000.0;

    n_set_jump_speed_ctrl(laserAIndex, jumpBits);
    HIM_DEBUG("ctrlSetJumpSpeed jumpBits --- {}",jumpBits);
    return true;
}

void RTC5Agent::listSetDelayMode(INT32 jumpDelay)
{
    // To disable this feature, the EdgeLevel value must be set to(2 32 –1)(default value)
    unsigned int edgeLevel = UINT_MAX;
    // 最小转角延时计算
    //if (sets.LayserEdgeLevelAng < sets.LayserSkyWritingAngle) {
    //    double d = (1.0 - Math.Cos(sets.LayserEdgeLevelAng * Math.PI / 180.0)) * sets.LayserPolyDelay;
    //    edgeLevel = Convert.ToUInt32(Math.Ceiling(d / 10.0));
    //}
    //时滞
    n_set_delay_mode_list(laserAIndex,
                          1U, //Enables the variable polygon delay mode
                          0, //DirectMove3D
                          edgeLevel, // 1 bit equals 10μs.
                          jumpDelay / 10, // 最小空跑延时, 1bit=10us, 不大于jump delay. sets.LayserMinJumpDelay,
                          0  // JumpLengthLimit = 0 disables the Variable Jump Delaymode
                          );
    HIM_INFO("PCIe Delay mode EdgeLevel: {}",edgeLevel);
};

void RTC5Agent::listSetDelayTime(INT32 laserOnDelay , INT32 laserOffDelay ,
                                 INT32 laserJumpDelay , INT32 laserMarkDelay , INT32 laserPolyDelay)
{
    // The laser delays are specified in units of 0.5 μs
    //SetLaserDelay(sets.LayserOnDelay, sets.LayserOffDelay);
    n_set_laser_delays(laserAIndex, laserOnDelay * 2, laserOffDelay * 2);


    //  the scanner delays (jump delay,  mark delay and polygon delay) are specified in units of 10 μs
    //SetScanDelay(sets.LayserJumpDelay, sets.LayserMarkDelay, sets.LayserPolyDelay);
    n_set_scanner_delays(laserAIndex,
                         laserJumpDelay / 10,
                         laserMarkDelay / 10,
                         laserPolyDelay / 10);
}

void RTC5Agent::listSetSkyWritring( INT32 laserSkyWritingTimelag,INT32 laserSkyWritingNprev,
                                    INT32 laserSkyWritingOnShift  ,INT32 laserSkyWritingNpost ,INT32 laserSkyWritingAngle )
{
    //以前设置过参数，可以直接开启
    // < 1/8: Sky Writing is deactivated,
    // //From 1/8 to 1/4, Sky Writing is activated, but Timelag = 0 is internally applied
    if(!skyWritingOn){
        laserSkyWritingTimelag = 0 ;
    }


    int val = 0;
    //LaserOnShift time(in μs) in Sky Writing mode 2 and 3 by no more than(10 × Npost – Timelag).
    if (laserSkyWritingOnShift >laserSkyWritingNpost  - laserSkyWritingTimelag){
        val = laserSkyWritingNpost  - laserSkyWritingTimelag;
        HIM_WARN("飞行扫描(OnShift)设置太长, 当前:" + std::to_string(laserSkyWritingOnShift)
                 + " us, 已自动修改为:" + std::to_string(val));
        SYSTEMCONFIG.setLongValue(PARAMETER,"layserSkyWritingOnShift",val);
    }
    // 时间参数
    n_set_sky_writing_para_list(laserAIndex,
                                laserSkyWritingTimelag, // 1.0 equals 1 µs  ≥ 1/4: Sky Writing mode 1 is activated
                                laserSkyWritingOnShift * 2, // 1 Bit equals 0.5 μs
                                (unsigned int)(laserSkyWritingNprev  / 10), //1 bit equals 10 μs, 10 × Nprev(Mod 2 or 3)
                                (unsigned int)(laserSkyWritingNpost  / 10));
    /*
               RTC5Wrap.n_set_sky_writing(this.laserAIndex,
                   sets.LayserSkyWritingTimelag,
                   sets.LayserSkyWritingOnShift * 2);*/

    // 角度限制 Limit is the cosine of the angular limit,
    //  The initialized value (after program start) is Limit = 0 (angular limit = 90°)
    double valD = cos(laserSkyWritingAngle *MATH_PI/180.0);
    n_set_sky_writing_limit_list(laserAIndex, valD);

    if (abs(laserSkyWritingTimelag) < 0.125){
        n_set_sky_writing_mode_list(laserAIndex, 0);
    }
    else{
        // 模式设置 > 2:Sky Writing mode 3 is activated.
        n_set_sky_writing_mode_list(laserAIndex, 3);
    }

    HIM_INFO("PCIe [{}] Timelag: {}, Shift: {}, Limit: {} , Nprev: {}, Npost: {} , {}",laserAIndex,
             laserSkyWritingTimelag,
             laserSkyWritingOnShift,
             laserSkyWritingAngle,
             laserSkyWritingNprev,
             laserSkyWritingNpost,
             valD);
}

void RTC5Agent::listSetMarkSpeed(double mark)
{
    // 转为bits/ms
    double speedBits = mark * kFactor / 1000.0;

    //RTC5Wrap.set_mark_speed(2000);
    //bits/ms set_mark_speed_ctrl
    n_set_mark_speed(laserAIndex, speedBits);
    CheckError();
}

void RTC5Agent::listSetLaserPower(double value)
{
    //实时百分比调节
    //value = (int)(powLaserMul * value / 100.0);
    //限制最大功率百分比
    if (value > laserPowerMax){
        HIM_WARN("Laser Power > {}", laserPowerMax);
        value = laserPowerMax*0.8;
    }

    int v =  value* 4095 / laserPowerMax;
    //12-bit output value for the ANALOG OUT1 analog output port as an unsigned
    //32 - bit value.Higher bits are ignored.
    //Value = 0 corresponds to an output value of 0 V.
    //Value = 2^12–1 corresponds to an output value of 10 V.
    n_write_da_1_list(laserAIndex, (unsigned int)v);

    laserCurrentPower = v;
    HIM_DEBUG("listSetLaserPower laserCurrentPower ---{} , {}" , value ,laserCurrentPower);

}

bool RTC5Agent::getLaserError()
{
    //<+>没有接线 暂时不用
    /*
          * Bit #0 DIGITAL IN1. 激光器故障输出信号
          * Bit #1 DIGITAL IN2. 激光器准备好
          */
    unsigned int st = n_get_laser_pin_in(laserAIndex);

    if ((st & 1) == 1){
        return true;
    }
    return false;
}

void RTC5Agent::MarkStart()
{
    unsigned int error = 0;
    unsigned int pos = 0;
    unsigned int st;

    //交替LIST1 LIST2
    if (listMark == 1U){
        pos = LIST1_2_CONUT + 1;
        listMark = 2U;
    }else{
        listMark = 1U;
    }

    /*while (0 == RTC5Wrap.load_list(listMark, pos)) {
           //  wait for list 1/2 to be not busy
           //Log.Info("wait for list to be not busy");
           Thread.Sleep(50);
       }*/
    //Bit #4    BUSY1
    //Bit #5    BUSY2
    st = (unsigned int)(listMark == 1U ? 0b00010000 : 0b00100000);
    while ((st & read_status()) != 0){
        Sleep(50);
        error = read_status();
    }

    set_start_list_pos(listMark, pos);
    error = get_last_error();
    if (0 != error){
        HIM_ERROR("set start list err :" + std::to_string(error));
    }
}

unsigned int RTC5Agent::MarkEnd()
{
    unsigned int error = 0;
    unsigned int st;

    error = get_last_error();
    if (0 != error){
        HIM_ERROR("sub call err :"+ std::to_string(error));
    }
    st = read_status();

    set_end_of_list();
    error = get_last_error();
    if (0 != error){
        HIM_ERROR("set end list err :"+ std::to_string(error));
    }
    st = read_status();

    //等待扫描结束
    st = 0b00110000;
    while (IsMarking()){
        Sleep(20);
    }

    execute_list(listMark);
    error = get_last_error();
    if (0 != error){
        HIM_ERROR("execute list err :" + std::to_string(error)
                  + " i:" + std::to_string(listMark)
                  + " s:" + std::to_string(read_status()));

        while ((unsigned int)RTC5_BUSY  == error)
        {
            Sleep(20);
            execute_list(listMark);
            error = get_last_error();
        }
    }
    //st = RTC5Wrap.read_status();
    //Log.Info("Mark Start" + nIndex + " s:" + st);
    return error;
}

void RTC5Agent::MakePause()
{
    pause_list();
}

void RTC5Agent::MakeResume()
{
    restart_list();
}

bool RTC5Agent::MarkStop()
{
    if (initSuccess&&IsMarking()){
        stop_execution();
    }
    return true;
}

bool RTC5Agent::scanSlicelayer()
{
    if (!initSuccess)
        return false;

    MarkStart();
    int commandCount{0};

    QSet<int> skipIndexs = TASKAGENT.getSkipIndexs();

    for(auto i : CLIREAD.getCliCurruntLayerData()->hatchesList){

        if(commandCount > LIST_SWITCH){
            HIM_DEBUG("scanSlicelayer commandCount full --- {}",commandCount);
            MarkEnd();

            do{
                Sleep(20);
            }while (IsMarking());

            MarkStart();
            commandCount = 0;
            HIM_DEBUG("scanSlicelayer commandCount clear --- {}",commandCount);
        }

        if(!skipIndexs.contains(i.label)){

            double laserSpeed = i.speed * TASKAGENT.getAdjLaserSpeedRatio();
            listSetMarkSpeed(laserSpeed);
            double laserPower = i.power * TASKAGENT.getAdjLaserPowerRatio();
            listSetLaserPower(laserPower);
            commandCount += 2;
            bool frontConnected{false};

            for(auto it = i.hatchVector.begin() ; it != i.hatchVector.end() ; ++it){
                bool nextConnected{false};
                if( it+1 != i.hatchVector.end()){
                    if(abs(it->second.rx() - (it+1)->first.rx()) < MY_ESP
                            && abs(it->second.ry() - (it+1)->first.ry()) < MY_ESP){
                        nextConnected = true;
                    }
                    if(frontConnected){
                        //mark
                        markAbs(laserAIndex,it->second.rx(),it->second.ry());
                        HIM_DEBUG("scanSlicelayer markAbs A --- {},{}",it->second.rx(),it->second.ry());
                        commandCount+=2;
                        if(!nextConnected){
                            frontConnected = false;
                        }
                    }else{
                        //jump
                        jumpAbs(laserAIndex,it->first.rx(),it->first.ry());
                        HIM_DEBUG("scanSlicelayer jumpAbs B--- {},{}",it->first.rx(),it->first.ry());
                        if(nextConnected){
                            //mark
                            markAbs(laserAIndex,it->second.rx(),it->second.ry());
                            HIM_DEBUG("scanSlicelayer markAbs B--- {},{}",it->second.rx(),it->second.ry());
                            frontConnected = true;
                        }else{
                            //应用功率衰减
                            double lineLength = sqrt(pow(it->second.rx() - it->first.rx() , 2) + pow(it->second.ry() - it->first.ry(),2));
                            if(lineLength<=0.2){
                                listSetLaserPower(laserPower*laserPowerByLen0d2);
                                HIM_DEBUG("scanSlicelayer laserPowerByLen0d2 C--- {}",laserPower*laserPowerByLen0d2);
                            }else if(lineLength<=0.5&&lineLength>0.2){
                                listSetLaserPower(laserPower*laserPowerByLen0d5);
                                HIM_DEBUG("scanSlicelayer laserPowerByLen0d5 C--- {}",laserPower*laserPowerByLen0d5);
                            }else if(lineLength<=1.0&&lineLength>0.5){
                                listSetLaserPower(laserPower*laserPowerByLen1d0);
                                HIM_DEBUG("scanSlicelayer laserPowerByLen1d0 C--- {}",laserPower*laserPowerByLen1d0);
                            }else if (lineLength<=2.0&&lineLength>1.5){
                                listSetLaserPower(laserPower*laserPowerByLen2d0);
                                HIM_DEBUG("scanSlicelayer laserPowerByLen2d0 C--- {}",laserPower*laserPowerByLen2d0);
                            }else if (lineLength>2.0){
                                listSetLaserPower(laserPower);
                            }
                            //mark
                            markAbs(laserAIndex,it->second.rx(),it->second.ry());
                            HIM_DEBUG("scanSlicelayer markAbs C--- {},{}",it->second.rx(),it->second.ry());
                        }

                        commandCount+=3;
                    }
                }else{
                    if(frontConnected){
                        //mark
                        markAbs(laserAIndex,it->second.rx(),it->second.ry());
                        HIM_DEBUG("scanSlicelayer markAbs D--- {},{}",it->second.rx(),it->second.ry());
                        commandCount+=2;
                    }else{
                        //jump
                        jumpAbs(laserAIndex,it->first.rx(),it->first.ry());
                        HIM_DEBUG("scanSlicelayer jumpAbs E--- {},{}",it->first.rx(),it->first.ry());
                        //应用功率衰减
                        double laserPower = i.power * TASKAGENT.getAdjLaserPowerRatio();
                        double lineLength = sqrt(pow(it->second.rx() - it->first.rx() , 2) + pow(it->second.ry() - it->first.ry(),2));
                        if(lineLength<=0.2){
                            listSetLaserPower(laserPower*laserPowerByLen0d2);
                            HIM_DEBUG("scanSlicelayer laserPowerByLen0d2 E--- {}",laserPower*laserPowerByLen0d2);
                        }else if(lineLength<=0.5&&lineLength>0.2){
                            listSetLaserPower(laserPower*laserPowerByLen0d5);
                            HIM_DEBUG("scanSlicelayer laserPowerByLen0d5 E--- {}",laserPower*laserPowerByLen0d5);
                        }else if(lineLength<=1.0&&lineLength>0.5){
                            listSetLaserPower(laserPower*laserPowerByLen1d0);
                            HIM_DEBUG("scanSlicelayer laserPowerByLen1d0 E--- {}",laserPower*laserPowerByLen1d0);
                        }else if (lineLength<=2.0&&lineLength>1.5){
                            listSetLaserPower(laserPower*laserPowerByLen2d0);
                            HIM_DEBUG("scanSlicelayer laserPowerByLen2d0 E--- {}",laserPower*laserPowerByLen2d0);
                        }else if (lineLength>2.0){
                            listSetLaserPower(laserPower);
                        }
                        //mark
                        markAbs(laserAIndex,it->second.rx(),it->second.ry());
                        HIM_DEBUG("scanSlicelayer markAbs --- E{},{}",it->second.rx(),it->second.ry());
                    }
                }
            }
        }
    }
    MarkEnd();
    return true;
}

bool RTC5Agent::IsMarking()
{
    unsigned int st;
    if (!initSuccess)
        return false;

    //等待扫描结束
    st = 0b00110000;
    return ((st & read_status()) != 0);
}

void RTC5Agent::testLensPara(QMap<QString,double> paraList)
{
    if (!initSuccess)
        return ;

    if(paraList.size()!=25){

        return;
    }else{
        kFactor = paraList["laserKFactor"];
        ctlSetFrequency(paraList["laserFrequency"],paraList["laserPulseWidth"]);
        ctlSetScaleAngle(paraList["laserXScale"], paraList["laserYScale"], paraList["laserXYrotate"]);
        ctlSetOffsetPosition(paraList["laserXmove"], paraList["laserYmove"], paraList["laserXYrotate"]);
        ctrlSetJumpSpeed(paraList["laserJumpSpeed"]);
        MarkStart();
        listSetDelayMode(paraList["laserJumpDelay"]);
        listSetDelayTime(paraList["laserOnDelay"],paraList["laserOffDelay"],
                paraList["laserJumpDelay"],paraList["laserMarkDelay"],paraList["laserPolyDelay"]);
        listSetSkyWritring(paraList["laserSkyWritingTimelag"],paraList["laserSkyWritingNprev"]
                ,paraList["laserSkyWritingOnShift"],paraList["laserSkyWritingNpost"],paraList["laserSkyWritingAngle"]);
        listSetMarkSpeed(paraList["laserMarkSpeed"]);
        listSetLaserPower(paraList["laserTestPower"]);
        jumpAbs(laserAIndex,0,CROSSSIZE);
        markAbs(laserAIndex,0,-CROSSSIZE);
        jumpAbs(laserAIndex,CROSSSIZE,0);
        markAbs(laserAIndex,-CROSSSIZE,0);
        jumpAbs(laserAIndex,0,0);
        MarkEnd();
    }
}

void RTC5Agent::testCalibrationPara(QMap<QString,double> paraList)
{
    if (!initSuccess){
        HIM_DEBUG("testCalibrationPara fail --- initSuccess is false");
        return ;
    }

    if(paraList.size()!=30){
        HIM_DEBUG("testCalibrationPara fail --- paraList.size()!=30");
        return;
    }else{
        kFactor = paraList["laserKFactor"];
        ctlSetFrequency(paraList["laserFrequency"],paraList["laserPulseWidth"]);
        ctlSetScaleAngle(paraList["laserXScale"], paraList["laserYScale"], paraList["laserXYrotate"]);
        ctlSetOffsetPosition(paraList["laserXmove"], paraList["laserYmove"], paraList["laserXYrotate"]);
        ctrlSetJumpSpeed(paraList["laserJumpSpeed"]);
        MarkStart();
        listSetDelayMode(paraList["laserJumpDelay"]);
        listSetDelayTime(paraList["laserOnDelay"],paraList["laserOffDelay"],
                paraList["laserJumpDelay"],paraList["laserMarkDelay"],paraList["laserPolyDelay"]);
        listSetSkyWritring(paraList["laserSkyWritingTimelag"],paraList["laserSkyWritingNprev"]
                ,paraList["laserSkyWritingOnShift"],paraList["laserSkyWritingNpost"],paraList["laserSkyWritingAngle"]);
        listSetMarkSpeed(paraList["laserMarkSpeed"]);
        listSetLaserPower(paraList["laserTestPower"]);

        double r =  paraList["doubleSpinBox_R"];
        int rows =  paraList["spinBox_rows"];
        int cols =  paraList["spinBox_cols"];
        double rowsInt = paraList["spinBox_rowsInt"];
        double colsInt = paraList["spinBox_colsInt"];

        //        int segments = r*36;
        //        double angleIncrement = 2 * MATH_PI / segments;

        for(int i = 0 ; i < rows ; i++){
            for(int j = 0 ; j < cols ; j++){

                double cx = rowsInt*(rows-1)/2-i*rowsInt;
                double cy = colsInt*(cols-1)/2-j*colsInt;

                double PXY = 0.1;

                double x = -r, y;
                //添加上半圆
                while (true){
                    y = sqrt(r * r - x * x);
                    if (x > -r){
                        markAbs(laserAIndex,x + cx, y + cy);
                    }else{
                        jumpAbs(laserAIndex,x + cx, y + cy);
                    }

                    if (x < r){
                        x += PXY;
                        if (x > r)
                            x = r;
                    }else{
                        break;
                    }
                }
                //添加下半圆
                x = r - PXY;
                while (true){
                    y = -sqrt(r * r - x * x);
                    markAbs(laserAIndex,x + cx, y + cy);

                    if (x > -r)
                    {
                        x -= PXY;
                        if (x < -r)
                            x = -r;
                    }else{
                        break;
                    }
                }




                //                jumpAbs(laserAIndex,cx+r,cy);
                //                for (int i = 0; i <= segments; ++i) {
                //                    double angle = i * angleIncrement;
                //                    markAbs(laserAIndex,cx+r*cos(angle),cy+r*sin(angle));
                //                }
                //                markAbs(laserAIndex,cx+r,cy);
            }
        }

        jumpAbs(laserAIndex,-rowsInt*(rows-1)/2-r,-colsInt*(cols-1)/2-r-2);
        markAbs(laserAIndex,-rowsInt*(rows-1)/2+r,-colsInt*(cols-1)/2-r-2);
        jumpAbs(laserAIndex,0,0);
        MarkEnd();
    }
}

void RTC5Agent::testGoto(QMap<QString,double> paraList)
{
    if (!initSuccess)
        return ;
    if(paraList.size()!=27){

        return;
    }else{
        kFactor = paraList["laserKFactor"];
        ctlSetFrequency(paraList["laserFrequency"],paraList["laserPulseWidth"]);
        ctlSetScaleAngle(paraList["laserXScale"], paraList["laserYScale"], paraList["laserXYrotate"]);
        ctlSetOffsetPosition(paraList["laserXmove"], paraList["laserYmove"], paraList["laserXYrotate"]);
        ctrlSetJumpSpeed(paraList["laserJumpSpeed"]);
        MarkStart();
        listSetDelayMode(paraList["laserJumpDelay"]);
        listSetDelayTime(paraList["laserOnDelay"],paraList["laserOffDelay"],
                paraList["laserJumpDelay"],paraList["laserMarkDelay"],paraList["laserPolyDelay"]);
        listSetSkyWritring(paraList["laserSkyWritingTimelag"],paraList["laserSkyWritingNprev"]
                ,paraList["laserSkyWritingOnShift"],paraList["laserSkyWritingNpost"],paraList["laserSkyWritingAngle"]);
        listSetMarkSpeed(paraList["laserMarkSpeed"]);
        listSetLaserPower(paraList["laserTestPower"]);

        gotoXY(laserAIndex,paraList["laserXpos"],paraList["laserYpos"]);

        MarkEnd();
    }
}

void RTC5Agent::testTimedlight(QMap<QString,double> paraList)
{
    if (!initSuccess)
        return ;
    if(paraList.size()!=26){
        return;
    }else{
        kFactor = paraList["laserKFactor"];
        ctlSetFrequency(paraList["laserFrequency"],paraList["laserPulseWidth"]);
        ctlSetScaleAngle(paraList["laserXScale"], paraList["laserYScale"], paraList["laserXYrotate"]);
        ctlSetOffsetPosition(paraList["laserXmove"], paraList["laserYmove"], paraList["laserXYrotate"]);
        ctrlSetJumpSpeed(paraList["laserJumpSpeed"]);
        MarkStart();
        listSetDelayMode(paraList["laserJumpDelay"]);
        listSetDelayTime(paraList["laserOnDelay"],paraList["laserOffDelay"],
                paraList["laserJumpDelay"],paraList["laserMarkDelay"],paraList["laserPolyDelay"]);
        listSetSkyWritring(paraList["laserSkyWritingTimelag"],paraList["laserSkyWritingNprev"]
                ,paraList["laserSkyWritingOnShift"],paraList["laserSkyWritingNpost"],paraList["laserSkyWritingAngle"]);
        listSetMarkSpeed(paraList["laserMarkSpeed"]);
        listSetLaserPower(paraList["laserTestPower"]);
        laser_on_list((UINT32)(paraList["laserLightOutputTime"] * 100000));
        MarkEnd();
    }
}

void RTC5Agent::testLines(QMap<QString,double> paraList)
{
    if (!initSuccess)
        return ;
    if(paraList.size()!=32){

        return;
    }else{
        kFactor = paraList["laserKFactor"];
        ctlSetFrequency(paraList["laserFrequency"],paraList["laserPulseWidth"]);
        ctlSetScaleAngle(paraList["laserXScale"], paraList["laserYScale"], paraList["laserXYrotate"]);
        ctlSetOffsetPosition(paraList["laserXmove"], paraList["laserYmove"], paraList["laserXYrotate"]);
        ctrlSetJumpSpeed(paraList["laserJumpSpeed"]);
        MarkStart();
        listSetDelayMode(paraList["laserJumpDelay"]);
        listSetDelayTime(paraList["laserOnDelay"],paraList["laserOffDelay"],
                paraList["laserJumpDelay"],paraList["laserMarkDelay"],paraList["laserPolyDelay"]);
        listSetSkyWritring(paraList["laserSkyWritingTimelag"],paraList["laserSkyWritingNprev"]
                ,paraList["laserSkyWritingOnShift"],paraList["laserSkyWritingNpost"],paraList["laserSkyWritingAngle"]);
        listSetMarkSpeed(paraList["laserMarkSpeed"]);
        listSetLaserPower(paraList["laserTestPower"]);


        double startAX = paraList["laserLineTestAX"];
        double startAY = paraList["laserLineTestAY"];
        double startBX = paraList["laserLineTestBX"];
        double startBY = paraList["laserLineTestBY"];
        double lineSpacing = paraList["laserLineTestSpacing"];
        int lineCount = paraList["laserLineTestCount"];


        bool alongX = !paraList["comboBox_lineDir"];

        while (lineCount--) {
            jumpAbs(laserAIndex,startAX,startAY);
            markAbs(laserAIndex,startBX,startBY);
            if(alongX){
                startAX += lineSpacing;
                startBX += lineSpacing;
            }else{
                startAY += lineSpacing;
                startBY += lineSpacing;
            }
        }
        jumpAbs(laserAIndex,0,0);

        MarkEnd();
    }
}

void RTC5Agent::testCircle(QMap<QString,double> paraList)
{
    if (!initSuccess)
        return ;
    if(paraList.size()!=28){

        return;
    }else{
        kFactor = paraList["laserKFactor"];
        ctlSetFrequency(paraList["laserFrequency"],paraList["laserPulseWidth"]);
        ctlSetScaleAngle(paraList["laserXScale"], paraList["laserYScale"], paraList["laserXYrotate"]);
        ctlSetOffsetPosition(paraList["laserXmove"], paraList["laserYmove"], paraList["laserXYrotate"]);
        ctrlSetJumpSpeed(paraList["laserJumpSpeed"]);
        MarkStart();
        listSetDelayMode(paraList["laserJumpDelay"]);
        listSetDelayTime(paraList["laserOnDelay"],paraList["laserOffDelay"],
                paraList["laserJumpDelay"],paraList["laserMarkDelay"],paraList["laserPolyDelay"]);
        listSetSkyWritring(paraList["laserSkyWritingTimelag"],paraList["laserSkyWritingNprev"]
                ,paraList["laserSkyWritingOnShift"],paraList["laserSkyWritingNpost"],paraList["laserSkyWritingAngle"]);
        listSetMarkSpeed(paraList["laserMarkSpeed"]);
        listSetLaserPower(paraList["laserTestPower"]);

        double r = paraList["laserCircleTestR"];
        double cx = paraList["laserCircleTestX"];
        double cy = paraList["laserCircleTestY"];

        double PXY = 0.1;

        double x = -r, y;
        //添加上半圆
        while (true){
            y = sqrt(r * r - x * x);
            if (x > -r){
                markAbs(laserAIndex,x + cx, y + cy);
            }else{
                jumpAbs(laserAIndex,x + cx, y + cy);
            }

            if (x < r){
                x += PXY;
                if (x > r)
                    x = r;
            }else{
                break;
            }
        }
        //添加下半圆
        x = r - PXY;
        while (true){
            y = -sqrt(r * r - x * x);
            markAbs(laserAIndex,x + cx, y + cy);

            if (x > -r)
            {
                x -= PXY;
                if (x < -r)
                    x = -r;
            }else{
                break;
            }
        }
        jumpAbs(laserAIndex,0,0);
        MarkEnd();
    }
}

void RTC5Agent::testRectangle(QMap<QString,double> paraList)
{
    if (!initSuccess)
        return ;

    if(paraList.size()!=29){
        return;

    }else{
        kFactor = paraList["laserKFactor"];
        ctlSetFrequency(paraList["laserFrequency"],paraList["laserPulseWidth"]);
        ctlSetScaleAngle(paraList["laserXScale"], paraList["laserYScale"], paraList["laserXYrotate"]);
        ctlSetOffsetPosition(paraList["laserXmove"], paraList["laserYmove"], paraList["laserXYrotate"]);
        ctrlSetJumpSpeed(paraList["laserJumpSpeed"]);
        MarkStart();
        listSetDelayMode(paraList["laserJumpDelay"]);
        listSetDelayTime(paraList["laserOnDelay"],paraList["laserOffDelay"],
                paraList["laserJumpDelay"],paraList["laserMarkDelay"],paraList["laserPolyDelay"]);
        listSetSkyWritring(paraList["laserSkyWritingTimelag"],paraList["laserSkyWritingNprev"]
                ,paraList["laserSkyWritingOnShift"],paraList["laserSkyWritingNpost"],paraList["laserSkyWritingAngle"]);
        listSetMarkSpeed(paraList["laserMarkSpeed"]);
        listSetLaserPower(paraList["laserTestPower"]);

        double aX = paraList["laserRectangleTestAX"];
        double aY = paraList["laserRectangleTestAY"];
        double bX = paraList["laserRectangleTestBX"];
        double bY = paraList["laserRectangleTestBY"];

        jumpAbs(laserAIndex , aX, aY);
        markAbs(laserAIndex , aX, bY);
        markAbs(laserAIndex , bX, bY);
        markAbs(laserAIndex , bX, aY);
        markAbs(laserAIndex , aX, aY);

        MarkEnd();
    }
}

void RTC5Agent::testSlicelayer(QMap<QString,double> paraList)
{
    if (!initSuccess){
        HIM_DEBUG("testSlicelayer fail --- initSuccess is false");
        return ;
    }

    if(paraList.size()!=25){
        HIM_DEBUG("testSlicelayer fail --- paraList.size()!=25 --- {}",paraList.size());
        return;
    }else{
        kFactor = paraList["laserKFactor"];
        ctlSetFrequency(paraList["laserFrequency"],paraList["laserPulseWidth"]);
        ctlSetScaleAngle(paraList["laserXScale"], paraList["laserYScale"], paraList["laserXYrotate"]);
        ctlSetOffsetPosition(paraList["laserXmove"], paraList["laserYmove"], paraList["laserXYrotate"]);
        ctrlSetJumpSpeed(paraList["laserJumpSpeed"]);
        MarkStart();
        listSetDelayMode(paraList["laserJumpDelay"]);
        listSetDelayTime(paraList["laserOnDelay"],paraList["laserOffDelay"],
                paraList["laserJumpDelay"],paraList["laserMarkDelay"],paraList["laserPolyDelay"]);
        listSetSkyWritring(paraList["laserSkyWritingTimelag"],paraList["laserSkyWritingNprev"]
                ,paraList["laserSkyWritingOnShift"],paraList["laserSkyWritingNpost"],paraList["laserSkyWritingAngle"]);
        listSetMarkSpeed(paraList["laserMarkSpeed"]);
        listSetLaserPower(paraList["laserTestPower"]);

        double laserPowerByLen2d0 = paraList["laserPowerByLen2d0"];
        double laserPowerByLen1d0 = paraList["laserPowerByLen1d0"];
        double laserPowerByLen0d5 = paraList["laserPowerByLen0d5"];
        double laserPowerByLen0d2 = paraList["laserPowerByLen0d2"];

        HIM_DEBUG("testSlicelayer laserPowerByLen2d0 --- {}",laserPowerByLen2d0);
        HIM_DEBUG("testSlicelayer laserPowerByLen1d0 --- {}",laserPowerByLen1d0);
        HIM_DEBUG("testSlicelayer laserPowerByLen0d5 --- {}",laserPowerByLen0d5);
        HIM_DEBUG("testSlicelayer laserPowerByLen0d2 --- {}",laserPowerByLen0d2);

        int commandCount{500};

        QSet<int> skipIndexs = TASKAGENT.getSkipIndexs();

        for(auto i : CLIREAD.getCliCurruntLayerData()->hatchesList){

            if(commandCount > LIST_SWITCH){
                HIM_DEBUG("testSlicelayer commandCount full --- {}",commandCount);
                MarkEnd();

                do{
                    Sleep(20);
                }while (IsMarking());

                MarkStart();
                commandCount = 0;
                qDebug()<<"commandCount clear:" << commandCount;
            }

            if(!skipIndexs.contains(i.label)){

                double laserSpeed = i.speed * TASKAGENT.getAdjLaserSpeedRatio();
                listSetMarkSpeed(laserSpeed);
                double laserPower = i.power * TASKAGENT.getAdjLaserPowerRatio();
                listSetLaserPower(laserPower);
                commandCount += 2;
                bool frontConnected{false};

                for(auto it = i.hatchVector.begin() ; it != i.hatchVector.end() ; ++it){
                    bool nextConnected{false};
                    if( it+1 != i.hatchVector.end()){
                        if(abs(it->second.rx() - (it+1)->first.rx()) < MY_ESP
                                && abs(it->second.ry() - (it+1)->first.ry()) < MY_ESP){
                            nextConnected = true;
                        }
                        if(frontConnected){
                            //mark                            
                            markAbs(laserAIndex,it->second.rx(),it->second.ry());
                            HIM_DEBUG("testSlicelayer markAbs A --- {},{}",it->second.rx(),it->second.ry());
                            commandCount+=2;
                            if(!nextConnected){
                                frontConnected = false;
                            }
                        }else{
                            //jump
                            jumpAbs(laserAIndex,it->first.rx(),it->first.ry());
                            HIM_DEBUG("testSlicelayer jumpAbs B--- {},{}",it->first.rx(),it->first.ry());
                            if(nextConnected){
                                //mark                               
                                markAbs(laserAIndex,it->second.rx(),it->second.ry());
                                HIM_DEBUG("testSlicelayer markAbs B--- {},{}",it->second.rx(),it->second.ry());
                                frontConnected = true;
                            }else{
                                //应用功率衰减                               
                                double lineLength = sqrt(pow(it->second.rx() - it->first.rx() , 2) + pow(it->second.ry() - it->first.ry(),2));
                                if(lineLength<=0.2){
                                    listSetLaserPower(laserPower*laserPowerByLen0d2);
                                    HIM_DEBUG("testSlicelayer laserPowerByLen0d2 C--- {}",laserPower*laserPowerByLen0d2);
                                }else if(lineLength<=0.5&&lineLength>0.2){
                                    listSetLaserPower(laserPower*laserPowerByLen0d5);
                                    HIM_DEBUG("testSlicelayer laserPowerByLen0d5 C--- {}",laserPower*laserPowerByLen0d5);
                                }else if(lineLength<=1.0&&lineLength>0.5){
                                    listSetLaserPower(laserPower*laserPowerByLen1d0);
                                    HIM_DEBUG("testSlicelayer laserPowerByLen1d0 C--- {}",laserPower*laserPowerByLen1d0);
                                }else if (lineLength<=2.0&&lineLength>1.5){
                                    listSetLaserPower(laserPower*laserPowerByLen2d0);
                                    HIM_DEBUG("testSlicelayer laserPowerByLen2d0 C--- {}",laserPower*laserPowerByLen2d0);
                                }else if (lineLength>2.0){
                                    listSetLaserPower(laserPower);
                                }
                                //mark
                                markAbs(laserAIndex,it->second.rx(),it->second.ry());
                                HIM_DEBUG("testSlicelayer markAbs C--- {},{}",it->second.rx(),it->second.ry());
                            }

                            commandCount+=3;
                        }
                    }else{
                        if(frontConnected){
                            //mark                          
                            markAbs(laserAIndex,it->second.rx(),it->second.ry());
                            HIM_DEBUG("testSlicelayer markAbs D--- {},{}",it->second.rx(),it->second.ry());
                            commandCount+=2;
                        }else{
                            //jump
                            jumpAbs(laserAIndex,it->first.rx(),it->first.ry());
                            HIM_DEBUG("testSlicelayer jumpAbs E--- {},{}",it->first.rx(),it->first.ry());
                            //应用功率衰减
                            double laserPower = i.power * TASKAGENT.getAdjLaserPowerRatio();
                            double lineLength = sqrt(pow(it->second.rx() - it->first.rx() , 2) + pow(it->second.ry() - it->first.ry(),2));
                            if(lineLength<=0.2){
                                listSetLaserPower(laserPower*laserPowerByLen0d2);
                                HIM_DEBUG("testSlicelayer laserPowerByLen0d2 E--- {}",laserPower*laserPowerByLen0d2);
                            }else if(lineLength<=0.5&&lineLength>0.2){
                                listSetLaserPower(laserPower*laserPowerByLen0d5);
                                HIM_DEBUG("testSlicelayer laserPowerByLen0d5 E--- {}",laserPower*laserPowerByLen0d5);
                            }else if(lineLength<=1.0&&lineLength>0.5){
                                listSetLaserPower(laserPower*laserPowerByLen1d0);
                                HIM_DEBUG("testSlicelayer laserPowerByLen1d0 E--- {}",laserPower*laserPowerByLen1d0);
                            }else if (lineLength<=2.0&&lineLength>1.5){
                                listSetLaserPower(laserPower*laserPowerByLen2d0);
                                HIM_DEBUG("testSlicelayer laserPowerByLen2d0 E--- {}",laserPower*laserPowerByLen2d0);
                            }else if (lineLength>2.0){
                                listSetLaserPower(laserPower);
                            }
                            //mark
                            markAbs(laserAIndex,it->second.rx(),it->second.ry());
                            HIM_DEBUG("testSlicelayer markAbs --- E{},{}",it->second.rx(),it->second.ry());
                        }
                    }
                }
            }
        }
        MarkEnd();
    }
}

void RTC5Agent::gotoXY(UINT cardId,double x, double y)
{
    if(abs(x*2)>sizeXY||abs(y*2)>sizeXY){
        HIM_WARN("gotoXY fail sizeXY: {} , x : {} , y : {}",sizeXY,x,y);
        return;
    }
    n_goto_xy(cardId,round(x*kFactorX),round(y*kFactorY));
}

void RTC5Agent::jumpAbs(UINT cardId,double x, double y)
{
    if(abs(x*2)>sizeXY||abs(y*2)>sizeXY){
        HIM_WARN("jumpAbs fail sizeXY: {} , x : {} , y : {}",sizeXY,x,y);
        return;
    }
    n_jump_abs(cardId,round(x*kFactorX),round(y*kFactorY));
}

void RTC5Agent::markAbs(UINT cardId,double x, double y)
{
    if(abs(x*2)>sizeXY||abs(y*2)>sizeXY){
        HIM_WARN("markAbs fail sizeXY: {} , x : {} , y : {}",sizeXY,x,y);
        return;
    }
    n_mark_abs(cardId,round(x*kFactorX),round(y*kFactorY));
}

}
